// =====================================================
// ====               MPU6050                       ====
// =====================================================
unsigned char Re_buf[11],counter=0;
float a[3],w[3],Angle[3],T;
//======================================================
//==                  Motor programer                ===
//======================================================
//MOTOR(int (+/-)Left_PWM,int (+/-)Right_PWM)
//‘+’电机正转；‘—’电机反转
//PWM:范围0~255
//======================================================
int M_left1 =7;
int M_left2 =6;
int M_right1 =5;
int M_right2 =4;
 // =========================================================
// ===                    INTERRUPT                       ===
// ==========================================================
//External Interrupts: 2 (interrupt 0), 3 (interrupt 1)
//                     18 (interrupt 5),19 (interrupt 4)
//                     20 (interrupt 3), 21 (interrupt 2)
// ==========================================================
//增量式-M法码盘测速，原理如下：
//PinA:下降沿触发外部中断
//PinB:若A相下降沿触发,判断B相:若出现LOW电平,为正脉冲计数器加1
//                             若出现HIGH电平,为负脉冲计数器减1
//码盘统一成前进方向
// ==========================================================
#define PinA_left 18
#define PinB_left 22
#define PinA_right 19
#define PinB_right 23
int count_left;
int count_right;
//================================================
//===               Communication              ===
//================================================
#define   LINE_END              10                             // \n
#define   SPLIT                 58                             // :
int STD_LOOP_TIME  = 10;             
int lastLoopTime = STD_LOOP_TIME;
int lastLoopUsefulTime = STD_LOOP_TIME;
unsigned long loopStartTime = 0;
int updateRate = 0;            //Nr of loop to skip sending and receving info from PC
//================================================
//===               Power_sample               ===
//================================================
float Value_filter;
float Voltage;
char OK = 1;
//=========================================================
//===                  PID_CONTROLLER                   ===
//=========================================================
int Pwm_out = 0;
int Turn_Need = 0;
int Speed_Need = 0;
float PWM_revise = 0;
int motorOffsetR , motorOffsetL;
int Item_limited = 1200;
float speeds , speeds_filter, positions;
//=========================================================
#define filter_p 0.3
float Kap = 23.2;
float Kad = 9.58;
float Ksp = 2.25;
float Ksi = 0.102;
//=================================================
void PID_PWM()
{
  //PWM_revise = 11.0 / Value_filter;

  speeds=(count_left + count_right) * 0.5;
  
  count_left = 0;
  count_right = 0;

  speeds_filter *=(1 - filter_p);  //速度互补滤波防止抖动
  speeds_filter +=speeds*filter_p;

  positions += speeds_filter;
  positions += Speed_Need;
  positions = constrain(positions, -Item_limited, Item_limited);//抗积分饱和

  Pwm_out = Kap*Angle[0] + Kad*w[0] ;
  Pwm_out += Ksp*speeds_filter + Ksi*positions ;  //PID控制器
  //Pwm_out *= PWM_revise;  //Power revise

  motorOffsetR = int(Pwm_out + Turn_Need);
  motorOffsetL = int(Pwm_out - Turn_Need);

  Motor(motorOffsetL , motorOffsetR);
}
//================================================
//===                  Setup                   ===
//================================================

void setup() {
  analogReference(EXTERNAL);   //使用外部电压基准
  Serial.begin(115200);
  Serial3.begin(115200);
  pinMode(M_left1, OUTPUT); //L298P直流电机驱动板的控制端口设置为输出模式
  pinMode(M_left2, OUTPUT);
  pinMode(M_right1, OUTPUT);
  pinMode(M_right2, OUTPUT);
  attachInterrupt(5, Code_left, FALLING);//小车左车轮编码器下降沿触发(外中断)
  attachInterrupt(4, Code_right, FALLING);
  delay(500);
}
//================================================
//===                  Loop                    ===
//================================================

void loop()
{
// ********************* Sensor aquisition & filtering *******************
  serialEvent();
  Power_sample();
// *********************** PID and motor drive *****************
  PID_PWM();
// ********************* print Debug info *************************************
  serialIn_GUI();       //Processing information from a pc
  serialOut_GUI();	//Sending information to pc for debug
// *********************** loop timing control **************************
  lastLoopUsefulTime = millis()-loopStartTime;
  if(lastLoopUsefulTime<STD_LOOP_TIME)
    delay(STD_LOOP_TIME-lastLoopUsefulTime);
  lastLoopTime = millis() - loopStartTime;
  loopStartTime = millis();
}


